#include "modbus_rtu.h"

// 定义Modbus RTU缓冲区
uint8_t modbus_buffer[MODBUS_BUFFER_SIZE];

// 设备寄存器buf（寄存器中的数据均以大端存储）
// 0-1:获得的距离数据
// 2-3:设备的空高
// 4-63:保留
uint8_t MODBUS_REG[MODBUS_REG_SIZE];

// 接收到的从机地址
uint8_t slave_addr = 0x02;
// 功能码
uint8_t funcation_code;
// 起始地址
uint16_t start_address;
// 读取数量（2字节为一个基本单位）
uint16_t count;
// CRC码
uint16_t check_code = 0;
// CRC检查状态(0:验证失败，1：验证成功)
uint8_t check_status = 0;

// 读取寄存器数据
int modbus_read_reg(uint16_t start_addr, uint16_t length, uint8_t *dst_buf)
{
    if (length <= MODBUS_REG_SIZE)
    {
        // 将读取到的寄存器的值赋值到传入的buf中
        for (size_t i = 0; i < length; i++)
        {
            dst_buf[i] = MODBUS_REG[start_addr + i];
        }
        // 成功返回1
        return 1;
    }
    // 失败返回0
    return 0;
}
// 写入寄存器数据
int modbus_write_reg(uint16_t start_addr, uint16_t length, uint8_t *src_buf)
{
    // 将buf的值传入寄存器中
    if (length <= MODBUS_REG_SIZE)
    {
        // 将读取到的buf的值赋值到寄存器中
        for (size_t i = 0; i < length; i++)
        {
            MODBUS_REG[start_addr + i] = src_buf[i];
        }
        // 成功返回1
        return 1;
    }
    // 失败返回0
    return 0;
}

void Modbus_Read_Registers(uint16_t start, uint16_t count, uint8_t *buffer)
{
    // 将请求报文写入缓冲区
    modbus_buffer[0] = 0x01;
    modbus_buffer[1] = 0x03;
    modbus_buffer[2] = start >> 8;
    modbus_buffer[3] = start & 0xFF;
    modbus_buffer[4] = count >> 8;
    modbus_buffer[5] = count & 0xFF;
    u16 crc = Modbus_CRC16(modbus_buffer, 6); // 计算CRC
    modbus_buffer[6] = crc & 0xFF;
    modbus_buffer[7] = crc >> 8;

    // 发送请求报文
    // USART_SendData(USART1, modbus_buffer, 8);
    // printf("%s",modbus_buffer);

    // 等待数据的接收
    int timeout = 10000; // 超时
    while (!USART_GetFlagStatus(USART1, USART_FLAG_RXNE) && timeout--)
        ;

    // 接收响应数据
    int len = 0;
    if (USART_GetFlagStatus(USART1, USART_FLAG_RXNE)) // 如果有数据
    {
        modbus_buffer[len++] = USART_ReceiveData(USART1);
        while (USART_GetFlagStatus(USART1, USART_FLAG_RXNE))
            modbus_buffer[len++] = USART_ReceiveData(USART1);
    }

    // 解析数据
    if (len == (count * 2 + 5)) // 数据长度正确
    {
        if (modbus_buffer[0] == 0x01 && modbus_buffer[1] == 0x03) // 响应码正确
        {
            for (int i = 0; i < count * 2; i += 2)
            {
                buffer[i] = modbus_buffer[i + 3];     // 高字节
                buffer[i + 1] = modbus_buffer[i + 4]; // 低字节
            }
        }
        else // 响应码错误
        {
            // 处理错误
        }
    }
    else // 数据长度错误
    {
        // 处理错误
    }
}

// 组包发送报文
void modbus_send(uint8_t slave_addr, uint8_t funcation_code, uint16_t start_addr, uint8_t data_len)
{
    // 报文长度
    uint8_t msg_len = 0, i = 0;
    // 从机地址
    modbus_buffer[msg_len++] = slave_addr; // 0
    // 功能码
    modbus_buffer[msg_len++] = funcation_code; // 1
    // 根据功能码进行不同操作
    switch (funcation_code)
    {
    case 0x03:
        // 字节计数
        modbus_buffer[msg_len++] = data_len; // 2
        // 数据类型转换,将2字节按照大端存储的方式存入缓存中
        // for (i = 0; i < data_len; i++)
        //     modbus_buffer[msg_len++] = val[(data_len - 1) - i];
        modbus_read_reg(start_addr, data_len, modbus_buffer + msg_len);
        msg_len += data_len;

    default:
        break;
    }

    // 获取当前校验码
    uint16_t crc = check_CRC16(modbus_buffer, msg_len);
    // 将校验码的低八位通过小端字节序组包到缓存中
    modbus_buffer[msg_len++] = (crc & 0x00FF);
    // 将校验码的高八位通过大端字节序组包到缓存中
    modbus_buffer[msg_len++] = (crc >> 8);

#if DEBUG
    printf("send info:\n");
    printf("slave_addr:%02d\n", slave_addr);
    printf("funcation_code:%02d\n", funcation_code);
    printf("start_address:%04d\n", start_address);
    printf("data_len:%04d\n", data_len);
    printf("check_code:%04x\n", crc);
    printf("send:");
    for (i = 0; i < msg_len; i++)
        printf("%02x", modbus_buffer[i]);
    printf("\n");
#endif
    for (i = 0; i < msg_len; i++)
        printf("%c", modbus_buffer[i]);
    // 清空数据
    start_address = 0;
    funcation_code = 0;
    count = 0;
    memset(modbus_buffer, 0, MODBUS_BUFFER_SIZE);
    check_code = 0;
}

// 解析接收的报文
uint8_t modbus_recv(uint8_t *buffer)
{
    uint8_t byte_len = 0;
    // 从机地址
    if (buffer[byte_len++] != slave_addr)
    {
        // 若不为当前地址则推出解析并返回错误码
        return SLAVE_ADDR_ERROR;
    }
    // 功能码
    funcation_code = buffer[byte_len++];
    if (funcation_code > 19)
        return FUNCATION_CODE_ERROR;
    switch (funcation_code)
    {
    case 0x03: // 读取一个地址
        // 起始地址(2字节)
        start_address |= buffer[byte_len++] << 8;
        start_address |= buffer[byte_len++];
        // 读取数量(2字节为基本单位大小)
        count |= buffer[byte_len++] << 8;
        count |= buffer[byte_len++];
        // 读取CRC校验码(小端存储)
        check_code |= buffer[byte_len++] & 0x00ff;
        check_code |= buffer[byte_len++] << 8;

        //  检验CRC校验码
        if (check_CRC16(buffer, byte_len - 2) == check_code)
            return CHECK_SUCC;
        return CHECK_ERROR;
        break;
    case 0x06: // 写单个寄存器
        // 起始地址(2字节)
        start_address |= buffer[byte_len++] << 8;
        start_address |= buffer[byte_len++];
        // 写寄存器
        modbus_write_reg(start_address, 2, buffer + byte_len);
        // 判断当前的工作模式
        if (*(uint16_t *)(buffer + byte_len))
            set_measure_mode(DEPTH_MODE);
        else
            set_measure_mode(DISTANCE_MODE);
        byte_len += 2;
        // 读取CRC校验码(小端存储)
        check_code |= buffer[byte_len++] & 0x00ff;
        check_code |= buffer[byte_len++] << 8;

        //  检验CRC校验码
        if (check_CRC16(buffer, byte_len - 2) == check_code)
            // 返回设置空高标志
            return SET_HEIGHT;
        return CHECK_ERROR;

    default:
        break;
    }
}

// 获取CRC校验码
u16 Modbus_CRC16(uint8_t *data, int len)
{
    u16 crc = 0xFFFF;

    for (int i = 0; i < len; i++)
    {
        crc ^= data[i];
        for (int j = 0; j < 8; j++)
        {
            if (crc & 0x01)
                crc = (crc >> 1) ^ 0xA001;
            else
                crc >>= 1;
        }
    }

    return crc;
}

// 查表法计算CRC码
uint16_t check_CRC16(uint8_t *pucFrame, uint16_t usLen)
{
    // 高位CRC码
    static const uint8_t aucCRCHi[] = {
        0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
        0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
        0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
        0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
        0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
        0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
        0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
        0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
        0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
        0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
        0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
        0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
        0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
        0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
        0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
        0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
        0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
        0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
        0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
        0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
        0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
        0x00, 0xC1, 0x81, 0x40};

    // 低位CRC码
    static const uint8_t aucCRCLo[] = {
        0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06, 0x07, 0xC7,
        0x05, 0xC5, 0xC4, 0x04, 0xCC, 0x0C, 0x0D, 0xCD, 0x0F, 0xCF, 0xCE, 0x0E,
        0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09, 0x08, 0xC8, 0xD8, 0x18, 0x19, 0xD9,
        0x1B, 0xDB, 0xDA, 0x1A, 0x1E, 0xDE, 0xDF, 0x1F, 0xDD, 0x1D, 0x1C, 0xDC,
        0x14, 0xD4, 0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6, 0xD2, 0x12, 0x13, 0xD3,
        0x11, 0xD1, 0xD0, 0x10, 0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3, 0xF2, 0x32,
        0x36, 0xF6, 0xF7, 0x37, 0xF5, 0x35, 0x34, 0xF4, 0x3C, 0xFC, 0xFD, 0x3D,
        0xFF, 0x3F, 0x3E, 0xFE, 0xFA, 0x3A, 0x3B, 0xFB, 0x39, 0xF9, 0xF8, 0x38,
        0x28, 0xE8, 0xE9, 0x29, 0xEB, 0x2B, 0x2A, 0xEA, 0xEE, 0x2E, 0x2F, 0xEF,
        0x2D, 0xED, 0xEC, 0x2C, 0xE4, 0x24, 0x25, 0xE5, 0x27, 0xE7, 0xE6, 0x26,
        0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0, 0xA0, 0x60, 0x61, 0xA1,
        0x63, 0xA3, 0xA2, 0x62, 0x66, 0xA6, 0xA7, 0x67, 0xA5, 0x65, 0x64, 0xA4,
        0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 0x6F, 0x6E, 0xAE, 0xAA, 0x6A, 0x6B, 0xAB,
        0x69, 0xA9, 0xA8, 0x68, 0x78, 0xB8, 0xB9, 0x79, 0xBB, 0x7B, 0x7A, 0xBA,
        0xBE, 0x7E, 0x7F, 0xBF, 0x7D, 0xBD, 0xBC, 0x7C, 0xB4, 0x74, 0x75, 0xB5,
        0x77, 0xB7, 0xB6, 0x76, 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71, 0x70, 0xB0,
        0x50, 0x90, 0x91, 0x51, 0x93, 0x53, 0x52, 0x92, 0x96, 0x56, 0x57, 0x97,
        0x55, 0x95, 0x94, 0x54, 0x9C, 0x5C, 0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E,
        0x5A, 0x9A, 0x9B, 0x5B, 0x99, 0x59, 0x58, 0x98, 0x88, 0x48, 0x49, 0x89,
        0x4B, 0x8B, 0x8A, 0x4A, 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 0x8C,
        0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 0x82, 0x42, 0x43, 0x83,
        0x41, 0x81, 0x80, 0x40};

    uint8_t ucCRCHi = 0xFF;
    uint8_t ucCRCLo = 0xFF;
    int iIndex;

    while (usLen--)
    {
        iIndex = ucCRCLo ^ *(pucFrame++);
        ucCRCLo = (uint8_t)(ucCRCHi ^ aucCRCHi[iIndex]);
        ucCRCHi = aucCRCLo[iIndex];
    }
    return (uint16_t)(ucCRCHi << 8 | ucCRCLo);
}

// 主机字节序(小端)转网络字节序(大端)--long
uint32_t htonl(uint32_t hostlong)
{
    return ((hostlong << 24) & 0xff000000) | ((hostlong << 8) & 0x00ff0000) | ((hostlong >> 8) & 0x0000ff00) | ((hostlong >> 24) & 0x000000ff);
}
// 主机字节序(小端)转网络字节序(大端)--short
uint16_t htons(uint16_t hostshort)
{
    return (hostshort << 8) | (hostshort >> 8);
}

// 网络字节序(大端)转主机字节序(小端)--long
uint32_t ntohl(uint32_t netlong)
{
    return ((netlong << 24) & 0xff000000) | ((netlong << 8) & 0x00ff0000) | ((netlong >> 8) & 0x0000ff00) | ((netlong >> 24) & 0x000000ff);
}
// 网络字节序(大端)转主机字节序(小端)--short
uint16_t ntohs(uint16_t netshort)
{
    return (netshort << 8) | (netshort >> 8);
}
